﻿#include "ssQuaternion.h"
#include "ssAssert.h"

namespace StarSeeker
{

ssFLOAT4X::ssFLOAT4X(void)
{
	x = y = z = w = 0.f;
}
ssFLOAT4X::ssFLOAT4X(ssfloat fx, ssfloat fy, ssfloat fz, ssfloat fw)
{
	x = fx;
	y = fy;
	z = fz;
	w = fw;
}
ssFLOAT4X::ssFLOAT4X(const ssFLOAT4X &sf4)
{
	x = sf4.x;
	y = sf4.y;
	z = sf4.z;
	w = sf4.w;
}

ssFLOAT4X::ssFLOAT4X(const ssfloat* val)
{
	memcpy(m, val, sizeof(ssFLOAT4));
}

void ssFLOAT4X::operator=(const ssFLOAT4X &sf4)
{
	x = sf4.x;
	y = sf4.y;
	z = sf4.z;
	w = sf4.w;
}

ssFLOAT4X::operator ssfloat* ()
{
	return m;
}
ssFLOAT4X::operator const ssfloat* () const
{
	return m;
}

void ssFLOAT4X::Set(ssfloat fx, ssfloat fy, ssfloat fz, ssfloat fw)
{
	x = fx;
	y = fy;
	z = fz;
	w = fw;
}
void ssFLOAT4X::Set(const ssfloat *fVal)
{
	x = fVal[0];
	y = fVal[1];
	z = fVal[2];
	w = fVal[3];
}
void ssFLOAT4X::Zero()
{
	x = y = z = w = 0.f;
}

void ssFLOAT4X::SetAxisAngle(const ssFLOAT3& axis, ssfloat angle)
{
	ssfloat s = Math3D::Sin(angle*0.5f);
	x = axis.x*s;
	y = axis.y*s;
	z = axis.z*s;
	w = Math3D::Cos(angle*0.5f);
}

void ssFLOAT4X::GetAxisAngle(ssFLOAT3& axis, ssfloat& angle)
{
	ssfloat scale = Math3D::Sqrt(x * x + y * y + z * z);
	axis.x = x / scale;
	axis.y = y / scale;
	axis.z = z / scale;
	angle = Math3D::ACos(w) * 2.0f;
}

void ssFLOAT4X::SetEuler(ssfloat ex, ssfloat ey, ssfloat ez)
{
	ssfloat sx = Math3D::Sin(ex*0.5f);
	ssfloat cx = Math3D::Cos(ex*0.5f);
	ssfloat sy = Math3D::Sin(ey*0.5f);
	ssfloat cy = Math3D::Cos(ey*0.5f);
	ssfloat sz = Math3D::Sin(ez*0.5f);
	ssfloat cz = Math3D::Cos(ez*0.5f);
	x = sx*cy*cz + cx*sy*sz;
	y = cx*sy*cz - sx*cy*sz;
	z = cx*cy*sz - sx*sy*cz;
	w = cx*cy*cz + sx*sy*sz;
}

void ssFLOAT4X::GetEuler(ssfloat& ex, ssfloat& ey, ssfloat& ez)
{
	ex = Math3D::ATan2(2.0f*(w*x+y*z), 1.0f-2.0f*(x*x+y*y));
	ey = Math3D::ASin(2.0f*(w*y-z*x));
	ez = Math3D::ATan2(2.0f*(w*z + x*y), 1.0f - 2.0f*(y*y + z*z));
}

//点乘
ssfloat ssFLOAT4X::Dot(const ssFLOAT4X& val) const
{
	return x*val.x + y*val.y + z*val.z + w*val.w;
}
//叉乘
ssFLOAT4X ssFLOAT4X::Cross(const ssFLOAT4X& val) const
{
	ssFLOAT4X q;
	q.w = w*val.w - x*val.x - y*val.y - z*val.z;
	q.x = w*val.x + x*val.w + z*val.y - y*val.z;
	q.y = w*val.y + y*val.w + x*val.z - z*val.x;
	q.z = w*val.z + z*val.w + y*val.x - x*val.y;
	return q;
}
//近似0向量
bool ssFLOAT4X::IsZero(ssfloat eps)
{
	return (Math3D::Abs(x) < eps && Math3D::Abs(y) < eps && Math3D::Abs(z) < eps && Math3D::Abs(w) < eps);
}
//两个向量近似相等
bool ssFLOAT4X::IsApproximate(const ssFLOAT4X& val, ssfloat eps)
{
	return (Math3D::Abs(x - val.x) < eps && Math3D::Abs(y - val.y) < eps && Math3D::Abs(z - val.z) < eps && Math3D::Abs(w - val.w) < eps);
}
//单位化
void ssFLOAT4X::Identity()
{
	x = y = z = 0.f;
	w = 1.f;
}
//取反
void ssFLOAT4X::Against()
{
	x = -x;
	y = -y;
	z = -z;
	w = -w;
}
//求模
ssfloat ssFLOAT4X::Length() const
{
	return Math3D::Sqrt(x*x + y*y + z*z + w*w);
}
//单位化
void ssFLOAT4X::Normalize()
{
	ssfloat len = Length();
	SS_ASSERT(len != 0.0f);
	if (len != 0.0f)
	{
		x /= len;
		y /= len;
		z /= len;
		w /= len;
	}
}

ssFLOAT4X ssFLOAT4X::Conjugate()
{
	ssFLOAT4X q(-x, -y, -z, w);
	return q;
}

ssFLOAT4X ssFLOAT4X::Inverse()
{
	ssFLOAT4X q(Conjugate());
	ssfloat len = Length();
	SS_ASSERT(len != 0.0f);
	if (len != 0.0f)
	{
		q /= len;
	}
	return q;
}

ssFLOAT4X ssFLOAT4X::operator*(ssfloat val) const
{
	ssFLOAT4X q(x*val, y*val, z*val, w*val);
	return q;
}

ssFLOAT4X ssFLOAT4X::operator/(ssfloat val) const
{
	ssFLOAT4X q(x/val, y/val, z/val, w/val);
	return q;
}

void ssFLOAT4X::operator*=(ssfloat val)
{
	x *= val;
	y *= val;
	z *= val;
	w *= val;
}

void ssFLOAT4X::operator/=(ssfloat val)
{
	x /= val;
	y /= val;
	z /= val;
	w /= val;
}

}
